#ifndef _ARES_MAP
#define _ARES_MAP

#include <ros/ros.h>
#include <vector>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <tf/transform_datatypes.h>
#include "ares_pathing/path.h"
#include "drive_commands/twist_path.h"
#include "drive_commands/path_point.h"

#define PI 3.14159

using namespace tf;
using namespace std;
using namespace drive_commands;

class aresMap{
        bool botflag;
        bool goalflag;    

    public:
        static const float botW = 0.5;      //bot width in m
        static const float botL = 0.9;      //bot length in m
        static const float botFrontOffset = 0.1;  //distance from bot pt. to front of bot.        
        static const float maxHeadingChange = PI/4; //maximum angle the bot can change in one cell move
        static const float goalOffset = 0.0; //goal offset in meters
        
        ros::Time load_time;
        float res;
        unsigned int w, h;
        tf::Pose origin;
        cell bot, goal;
                
        Path * road;

        //CONSTRUCTORS
        aresMap(nav_msgs::OccupancyGrid msg);
        ~aresMap();
        
        /*Preconditions: a occupancy grid msg that represents a new map
          Postconditions: the map, bot and goal locations are updated for the new map
        */
        void updateMap(nav_msgs::OccupancyGrid msg);
        
        /*Preconditions: An odometry message of the robot position to update
          Postconditions: The saved position, orientation and velocity of the robot in the map is updated.
        */        
        void updateBot(nav_msgs::Odometry odom);
        
        /*Preconditions: a Pose representing a desired location
          Postconditions: the pose is saved as the current goal
        */        
        void setGoal(geometry_msgs::Pose goalIn);

        /*Preconditions: none
          Postconditions: an array of cells is returned that represents a path from the bot to the goal.
        */
        void layPath();

        /*Preconditions: none
          Postconditions: the velocities and directions for the next 5 path steps are released
        */
        void writeMsg(drive_commands::twist_path * m);
    
        /*Preconditions: a distance in the map
          Postconditions: the # of cells is returned
        */
        int metersToIndex(float m);

        //for debugging
        void printBot();
        void printGoal();
        void printWorld();

    private:
        vector<int> grid;

        /*Preconditions: A pose within the odom frame
          Postoconditions: A posInfo with all relevant position information is returned
        */
        cell poseToCell(tf::Pose p);

        cell offsetBackLinear(cell c);

        /*INLINE
          Preconditions: A cell in the map
          Postconditions: the index of the cell in grid vector is returned.
        */
        int coordToIndex(int xc,int yc){
            return (yc*w + xc);
        }

        /*INLINE - From the internet!
          Preconditions: a float
          Postconditions: the properly rounded (whether + or -) number
        */
        float round(float number){
            return number < 0.0 ? ceil(number - 0.5) : floor(number + 0.5);
        }

        /*
          Preconditions: a cell in the map
          Postconditions: true if the cell is unoccupied or unknown, false if occupied
        */
        bool footprintCheck(cell c);

        /*Preconditions: the coordinates of a cell in the map (in cells)
          Postconditions: true if the cell is unoccupied or unknown, false if occupied
        */
        bool spotCheck(int xc, int yc){
            ROS_INFO("Spot Checking (%d,%d)",xc,yc);
            int i = coordToIndex(xc,yc);
            if(i>=grid.size() || i<0){
                ROS_INFO("spot check outside map");
                return -1;
            }
            else if(grid[i] > 45){
                ROS_INFO("spot check fail: %i",grid[i]);                
                return false;
            }
            else{
                ROS_INFO("spot check pass!");
                return true;
            }
        }

        /*Preconditions: a cell to make a neighbor for, the dx & dy (in cells) to the neighbor from the cell
          Postconditions: a cell for the neighbor is returned, where the heading is the direction from the old cell to the new cell.
        */
        cell makeNeighborCell(cell c, int dx, int dy);


        /*Preconditions: the coordinates of a cell in the map, the goal, and the vector of paths, and the index of the current path
          Postconditions: returns a vector of the path indexes for which this cell is included. Recursively calls for its valid neighbors before returning.
        */   
        vector<int> checkNeighbors(cell c, cell g, vector<Path> * p);

        /*Preconditions: the cell who is checking its neighbor, the neighbor, the map goal, and a pointer to the array of paths
          Postconditions: the cell checks its status and creates a path or checks its neighbors accordingly. It adds itself to the proper paths and returns the indicies of the paths to which it belongs.
        */
        vector<int> checkNeighbor(cell c, cell n, cell g, vector<Path> * p);


};//end aresMap

#endif
